#include <Arduino.h>
#include "MIXER.h"
#include "hardwareDef.h"
#include "simpleMath.h"

int32_t outMix[6] = {1500, 1500, 988, 1500, 1500, 1500};
extern float mixer1[4], mixer2[4], mixer3[4], mixer4[4], mixer5[4];

void MIXER(int32_t in[4])
{
  outMix[0] = ((mixer1[0]*in[0] + mixer1[1]*in[1] + mixer1[2]*in[2] ) / 8) + trimCenter1;
  outMix[1] = ((mixer2[0]*in[0] + mixer2[1]*in[1] + mixer2[2]*in[2] ) / 8) + trimCenter2;
  outMix[2] = ((mixer3[0]*in[0] + mixer3[1]*in[1] + mixer3[2]*in[2] ) / 8) + trimCenter3;
  outMix[3] = ((mixer4[0]*in[0] + mixer4[1]*in[1] + mixer4[2]*in[2] ) / 8) + trimCenter4;
  outMix[4] = ((mixer5[0]*in[0] + mixer5[1]*in[1] + mixer5[2]*in[2] ) / 8) + trimCenter5;
  outMix[5] = 1500;
}

int32_t MIXER_VAL(uint8_t channel)
{
  return outMix[channel];
}
